// system
#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include <assert.h>
// local
#include "gtrack_int.h"
#include "gtrack.h"
#include "gtrack_queue.h"
// printf
#include "GtrackPrintf.h"
#include "typedef.h"

#define  MAX_NEIGHBORS 100   // 最大邻近点个数

const GTRACK_sceneryParams    defaultSceneryParams = { 0, { { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f } }, 0, { { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f } } }; /* no boundaries, no static boxes */
const GTRACK_gatingParams     defaultGatingParams = { 2.f, { 3.f, 2.f, 2.f, 0.f } };
const GTRACK_allocationParams defaultAllocationParams = { 100.f, 0.5f, 5U, 1.f, 2.f }; /* At least 100 SNR, 100 SNR when obscured, 0.5 m/s, 5 points: up to 1m in distance, up to 2m/c in velocity */
const GTRACK_mergeParams      defaultMergeParams = { 10U, 1.f, 1.f }; /* At least 10 count, 1.0 dist sub, 1.0 speed sub */
const GTRACK_unrollingParams  defaultUnrollingParams = { 0.5f, 0.1f };

const float zero3x3[9] = {
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f
};

const float pinit6x6[36] = {
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.5f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.5f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    1.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    0.f,
    1.f
};

void* gtrack_create(GTRACK_moduleConfig* config, int32_t* errCode)
{
    GtrackModuleInstance* inst = NULL;
    uint8_t               uid;
    float                 dt, dt2, dt3, dt4;
    float                 varX, varY, varZ;

    *errCode = GTRACK_EOK;

    if (config->maxNumPoints > GTRACK_NUM_POINTS_MAX)
    {
        *errCode = GTRACK_EINVAL;
        goto exit;
    }

    if (config->maxNumTracks > GTRACK_NUM_TRACKS_MAX)
    {
        *errCode = GTRACK_EINVAL;
        goto exit;
    }

    inst = (GtrackModuleInstance*)gtrack_alloc(1, sizeof(GtrackModuleInstance));
    if (inst == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    memset(inst, 0, sizeof(GtrackModuleInstance));

    inst->maxNumPoints = config->maxNumPoints;
    inst->maxNumTracks = config->maxNumTracks;

    inst->heartBeat = 0U;

    /* default parameters */
    inst->params.gatingParams = defaultGatingParams;
    inst->params.unrollingParams = defaultUnrollingParams;
    inst->params.allocationParams = defaultAllocationParams;
    inst->params.sceneryParams = defaultSceneryParams;
    inst->params.mergeParams = defaultMergeParams;

    if (config->advParams != NULL)
    {
        /* user overwrites default parameters */
        if (config->advParams->gatingParams)
            memcpy(&inst->params.gatingParams, config->advParams->gatingParams, sizeof(GTRACK_gatingParams));
        if (config->advParams->unrollingParams)
            memcpy(&inst->params.unrollingParams, config->advParams->unrollingParams, sizeof(GTRACK_unrollingParams));
        if (config->advParams->allocationParams)
            memcpy(&inst->params.allocationParams, config->advParams->allocationParams, sizeof(GTRACK_allocationParams));
        if (config->advParams->mergeParams)
            memcpy(&inst->params.mergeParams, config->advParams->mergeParams, sizeof(GTRACK_mergeParams));
        if (config->advParams->sceneryParams)
            memcpy(&inst->params.sceneryParams, config->advParams->sceneryParams, sizeof(GTRACK_sceneryParams));
    }

    inst->params.radarParams = config->radarParams;
    dt = config->radarParams.deltaT;
    dt2 = powf(dt, 2);
    dt3 = powf(dt, 3);
    dt4 = powf(dt, 4);

    /* initialize process variance to 1/2 of maximum target acceleration */
    memcpy(inst->params.maxAcceleration, config->maxAcceleration, sizeof(config->maxAcceleration));
    /* 将过程方差初始化为最大目标加速度的二分之一 */
    varX = powf(0.5f * config->maxAcceleration[0], 2);
    varY = powf(0.5f * config->maxAcceleration[1], 2);
    varZ = powf(0.5f * config->maxAcceleration[2], 2);

    /* configured parameters */
    switch (config->stateVectorType)
    {
    case GTRACK_STATE_VECTORS_2DA:
    {
        float F6[36] = {
            1.f,
            0.f,
            dt,
            0.f,
            dt2 / 2,
            0.f,
            0.f,
            1.f,
            0.f,
            dt,
            0.f,
            dt2 / 2,
            0.f,
            0.f,
            1.f,
            0.f,
            dt,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            dt,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f
        };

        float Q6[36] = {
            dt4 / 4 * varX,
            0.f,
            dt3 / 2 * varX,
            0.f,
            dt2 / 2 * varX,
            0.f,
            0.f,
            dt4 / 4 * varY,
            0.f,
            dt3 / 2 * varY,
            0.f,
            dt2 / 2 * varY,
            dt3 / 2 * varX,
            0.f,
            dt2 * varX,
            0.f,
            dt * varX,
            0.f,
            0.f,
            dt3 / 2 * varY,
            0.f,
            dt2 * varY,
            0.f,
            dt * varY,
            dt2 / 2 * varX,
            0.f,
            dt * varX,
            0.f,
            1.f * varX,
            0.f,
            0.f,
            dt2 / 2 * varY,
            0.f,
            dt * varY,
            0.f,
            1.f * varY
        };

        inst->params.F = (float*)gtrack_alloc(1, sizeof(F6));
        inst->params.Q = (float*)gtrack_alloc(1, sizeof(Q6));

        memcpy(inst->params.F, F6, sizeof(F6));
        memcpy(inst->params.Q, Q6, sizeof(Q6));
    }
    break;

    case GTRACK_STATE_VECTORS_3DA:
    {
        float F9[81] = {
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            dt2 / 2,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            dt2 / 2,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            dt2 / 2,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            dt,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            0.f,
            1.f
        };

        float Q9[81] = {
            dt4 / 4 * varX,
            0.f,
            0.f,
            dt3 / 2 * varX,
            0.f,
            0.f,
            dt2 / 2 * varX,
            0.f,
            0.f,
            0.f,
            dt4 / 4 * varY,
            0.f,
            0.f,
            dt3 / 2 * varY,
            0.f,
            0.f,
            dt2 / 2 * varY,
            0.f,
            0.f,
            0.f,
            dt4 / 4 * varZ,
            0.f,
            0.f,
            dt3 / 2 * varZ,
            0.f,
            0.f,
            dt2 / 2 * varZ,
            dt3 / 2 * varX,
            0.f,
            0.f,
            dt2 * varX,
            0.f,
            0.f,
            dt * varX,
            0.f,
            0.f,
            0.f,
            dt3 / 2 * varY,
            0.f,
            0.f,
            dt2 * varY,
            0.f,
            0.f,
            dt * varY,
            0.f,
            0.f,
            0.f,
            dt3 / 2 * varZ,
            0.f,
            0.f,
            dt2 * varZ,
            0.f,
            0.f,
            dt * varZ,
            dt2 / 2 * varX,
            0.f,
            0.f,
            dt * varX,
            0.f,
            0.f,
            1.f * varX,
            0.f,
            0.f,
            0.f,
            dt2 / 2 * varY,
            0.f,
            0.f,
            dt * varY,
            0.f,
            0.f,
            1.f * varY,
            0.f,
            0.f,
            0.f,
            dt2 / 2 * varZ,
            0.f,
            0.f,
            dt * varZ,
            0.f,
            0.f,
            1.f * varZ
        };

        inst->params.F = (float*)gtrack_alloc(1, sizeof(F9));
        inst->params.Q = (float*)gtrack_alloc(1, sizeof(Q9));

        memcpy(inst->params.F, F9, sizeof(F9));
        memcpy(inst->params.Q, Q9, sizeof(Q9));
    }
    break;

    default:
        *errCode = GTRACK_EINVAL;
        goto exit;
    }
    inst->params.stateVectorType = config->stateVectorType;
    inst->params.initialRadialVelocity = config->initialRadialVelocity;
    // 以下分配所需内存并初始化
    /* hTrack is an array of void pointers */
    inst->hTrack = (void**)gtrack_alloc(inst->maxNumTracks, sizeof(GtrackUnitInstance*));
    if (inst->hTrack == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    /* scoreSheet is an array of best scores */
    inst->bestScore = (float*)gtrack_alloc(inst->maxNumPoints, sizeof(float));
    if (inst->bestScore == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    inst->epsIndex = (float*)gtrack_alloc(inst->maxNumPoints, sizeof(float));
    if (inst->epsIndex == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }
    for (int n = 0; n < inst->maxNumPoints; n++)
        inst->epsIndex[n] = inst->params.allocationParams.maxDistanceThre;

    /* association array holds the ids of the best scorers */
    inst->bestIndex = (uint8_t*)gtrack_alloc(inst->maxNumPoints, sizeof(uint8_t));
    if (inst->bestIndex == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    /* allocation array holds the measurement indices of allocation set */
    inst->allocIndex = (uint16_t*)gtrack_alloc(inst->maxNumPoints, sizeof(uint16_t));
    if (inst->allocIndex == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    /* array of tracking IDs */
    inst->uidElem = (GTrack_ListElem*)gtrack_alloc(inst->maxNumTracks, sizeof(GTrack_ListElem));
    if (inst->uidElem == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    inst->associateNums = (uint16_t*)gtrack_alloc(inst->maxNumTracks, sizeof(uint16_t));
    if (inst->associateNums == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    inst->associateDatas = (uint16_t**)gtrack_alloc(inst->maxNumTracks, sizeof(uint16_t*));
    if (inst->associateDatas == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    inst->targetNumTotal = 0U;
    inst->targetNumCurrent = 0U;

    gtrack_listInit(&inst->freeList);
    gtrack_listInit(&inst->activeList);
    initializeQueue(&(inst->clusterQ), inst->maxNumPoints);

    /* Create unit trackers */
    for (uid = 0U; uid < inst->maxNumTracks; uid++)
    {
        // 记录当前tracker的UID到空闲链表中
        inst->uidElem[uid].data = uid;
        gtrack_listEnqueue(&inst->freeList, &inst->uidElem[uid]);
        // 创建UID为uid的tracker，加入到hTrack数组中
        inst->params.uid = uid;
        inst->hTrack[uid] = gtrack_unitCreate(&inst->params, errCode);
        inst->associateDatas[uid] = (uint16_t*)gtrack_alloc(100, sizeof(uint16_t));
        if (*errCode != GTRACK_EOK)
        {
            goto exit;
        }
    }

exit:
    if (*errCode != GTRACK_EOK)
    {
        if (inst != NULL)
        {
            gtrack_delete(inst);
        }
        inst = NULL;
    }

    return inst;
}

void gtrack_delete(void* handle)
{
    GtrackModuleInstance* inst;
    uint16_t              n;

    if (handle)
    {
        inst = (GtrackModuleInstance*)handle;
        for (n = 0; n < inst->maxNumTracks; n++)
        {
            if (inst->hTrack[n] != NULL)
            {
                gtrack_unitDelete(inst->hTrack[n]);
                gtrack_unitDelete(inst->associateDatas[n]);
            }
        }
        if (inst->hTrack)
            gtrack_free(inst->hTrack, inst->maxNumTracks * sizeof(GtrackUnitInstance*));
        if (inst->params.F)
            gtrack_free(inst->params.F, GTRACK_STATE_VECTOR_SIZE * GTRACK_STATE_VECTOR_SIZE * sizeof(float));
        if (inst->params.Q)
            gtrack_free(inst->params.Q, GTRACK_STATE_VECTOR_SIZE * GTRACK_STATE_VECTOR_SIZE * sizeof(float));
        if (inst->bestScore)
            gtrack_free(inst->bestScore, inst->maxNumPoints * sizeof(float));
        if (inst->bestIndex)
            gtrack_free(inst->bestIndex, inst->maxNumPoints * sizeof(uint8_t));
        if (inst->allocIndex)
            gtrack_free(inst->allocIndex, inst->maxNumPoints * sizeof(uint16_t));
        if (inst->uidElem)
            gtrack_free(inst->uidElem, inst->maxNumTracks * sizeof(GTrack_ListElem));
        if (inst->associateDatas)
            gtrack_free(inst->associateDatas, inst->maxNumTracks * sizeof(uint16_t*));
        if (inst->associateNums)
            gtrack_free(inst->associateNums, inst->maxNumTracks * sizeof(uint16_t));

        freeQueue(&(inst->clusterQ));
        gtrack_free(inst, sizeof(GtrackModuleInstance));
    }
}

#ifndef far
#define far /* Empty */
#endif

far unsigned int memoryBytesUsed = 0;

void* gtrack_alloc(uint32_t numElements, uint32_t sizeInBytes)
{
    memoryBytesUsed += numElements * sizeInBytes;
    return malloc(numElements * sizeInBytes);
}

void gtrack_free(void* pFree, uint32_t sizeInBytes)
{
    memoryBytesUsed -= sizeInBytes;
    free(pFree);
}

void gtrack_log(const char* format, ...)
{
    va_list args;
    va_start(args, format);
    vprintf(format, args);
    va_end(args);
}

void gtrack_step(void* handle, GTRACK_measurementPoint* point, GTRACK_measurement_vector* var, uint16_t mNum, GTRACK_targetDesc* t, uint16_t* tNum, uint8_t* mIndex)
{
    GtrackModuleInstance* inst = (GtrackModuleInstance*)handle;

//    inst->heartBeat++;
//    printf_message(BASIC_INFO_PRINT, "-------------------- No. %d frame ---------------------\n", inst->heartBeat);

    if (mNum > inst->maxNumPoints)
        mNum = inst->maxNumPoints;

    gtrack_modulePreprocess(inst, point, mNum);
    gtrack_modulePredict(inst);
    gtrack_moduleAssociate(inst, point, mNum);
    gtrack_moduleMerge(inst);
    gtrack_moduleUpdate(inst, point, mNum);
    gtrack_moduleReport(inst, t, tNum, point, mNum);
    gtrack_moduleDelete(inst);
}

void gtrack_modulePreprocess(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t mNum)
{
    uint16_t              n;
    uint16_t              numBoxes;
    GTRACK_cartesian_position pos;
    for (n = 0; n < mNum; n++)
    {
        inst->bestScore[n] = FLT_MAX;
        if (inst->params.sceneryParams.numBoundaryBoxes)
        {
            // 如果场景边界框存在，则先设置当前点位于框外
            inst->bestIndex[n] = GTRACK_ID_POINT_BEHIND_THE_WALL;
            // 测量向量的极坐标状态向量的转笛卡尔坐标
            gtrack_sph2cart(&point[n].vector, &pos);
            // SNR db转幅值比
            point[n].snr = powf(10, (point[n].snr * 0.05f));                    // 10 * log10(a^2 + b^2 + c^2 ...)
            point[n].doa_snr = powf(10, (point[n].doa_snr * 0.05f));     // 10 * log10(a + b + c ...)^2
            // 遍历边界框，如果点在某一边界框内，则设置为有效点
            for (numBoxes = 0; numBoxes < inst->params.sceneryParams.numBoundaryBoxes; numBoxes++)
            {
                if (gtrack_isPointInsideBox(&pos, &inst->params.sceneryParams.boundaryBox[numBoxes]))
                {
                    /* Valid point */
                    inst->bestIndex[n] = GTRACK_ID_POINT_NOT_ASSOCIATED;
                    break;
                }
            }
        }
        else
        {
            // 如果场景边界框不存在，直接设置当前点有效
            inst->bestIndex[n] = GTRACK_ID_POINT_NOT_ASSOCIATED;
        }
    }
}

void gtrack_modulePredict(GtrackModuleInstance* inst)
{
    GTrack_ListElem* tElem;
    uint16_t         uid;

    // 遍历活动链表中的tracker，执行每一个tracker的predict函数
    tElem = gtrack_listGetFirst(&inst->activeList);
    while (tElem != 0)
    {
        uid = tElem->data;
        if (uid > inst->maxNumTracks)
        {
            /* This should never happen */
            assert(0);
        }

        gtrack_unitPredict(inst->hTrack[uid]);

        tElem = gtrack_listGetNext(tElem);
        inst->associateNums[uid] = 0;
    }
}

uint16_t gtrack_moduleCluster(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t mNum, uint16_t initIdx, GTRACK_measurementPoint* sum)
{
    uint16_t allocNum, idx, k;
    GTRACK_measurementUnion mCenter, mCurrent;
    float dist;
    Queue* q = &(inst->clusterQ);
    resetQueue(q);
    allocNum = 0;
    sum->snr = 0;
    sum->doa_snr = 0;
    memset(sum, 0, sizeof(GTRACK_measurementPoint));
    enqueue(q, initIdx);
    inst->bestIndex[initIdx] = GTRACK_ID_POINT_TOO_WEAK;    // 已遍历的点标志初始化为weak，避免重复遍历，成功关联轨迹后赋予轨迹uid
    while (!isEmpty(q))
    {
        idx = dequeue(q);
        inst->allocIndex[allocNum] = idx;
        allocNum++;
        sum->snr += point[idx].snr;
        sum->doa_snr += point[idx].doa_snr;
        mCenter.vector = point[idx].vector;
        // 求和之前先进行径向速度展开
        mCenter.vector.doppler = gtrack_unrollRadialVelocity(inst->params.radarParams.maxRadialVelocity, point[initIdx].vector.doppler, mCenter.vector.doppler);
        gtrack_vectorScalarMulAcc(GTRACK_MEASUREMENT_VECTOR_SIZE, mCenter.array, point[idx].snr, sum->array); // 计算SNR加权位置之和
        
        for (k = 0; k < mNum; k++)
        {
            if (inst->bestIndex[k] == GTRACK_ID_POINT_NOT_ASSOCIATED)
            {
                mCurrent.vector = point[k].vector;
                mCurrent.vector.doppler = gtrack_unrollRadialVelocity(inst->params.radarParams.maxRadialVelocity, mCenter.vector.doppler, mCurrent.vector.doppler);
                if (fabsf(mCurrent.vector.doppler - mCenter.vector.doppler) < inst->params.allocationParams.maxVelThre)
                {
                    // 此处重复计算了距离、可优化，需评估耗时影响，耗时不大也可忽略
                    dist = gtrack_calcDistance(&mCenter.vector, &mCurrent.vector);
                    float minEps = inst->epsIndex[idx] < inst->epsIndex[k] ? inst->epsIndex[idx] : inst->epsIndex[k];// 修改部分
                    if (dist <= minEps)
                    {
                        enqueue(q, k);
                        inst->bestIndex[k] = GTRACK_ID_POINT_TOO_WEAK;    // 已遍历的点标志初始化为weak，避免重复遍历，成功关联轨迹后赋予轨迹uid
                    }
                }
            }
        }
    }
    return allocNum;
}

void gtrack_moduleAssociate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t num)
{
    uint16_t n, idx, allocNum;
    GTRACK_measurementPoint mCenter;
    GTRACK_measurementPoint mSum;
    GtrackUnitInstance* gUnit;
    bool ret;

    gtrack_moduleCalcAdaptiveEps(inst, point, num, 8);
    for (n = 0; n < num; n++)
    {
        if (inst->bestIndex[n] == GTRACK_ID_POINT_NOT_ASSOCIATED)
        {
            // 以当前点为初始点，进行聚类
            allocNum = gtrack_moduleCluster(inst, point, num, n, &mSum);
            // 聚类成功，关联/分配轨迹
            if (allocNum)
            {
                mCenter.snr = mSum.snr;
                mCenter.doa_snr = mSum.doa_snr;
                gtrack_vectorScalarMul(GTRACK_MEASUREMENT_VECTOR_SIZE, mSum.array, 1.0f / mSum.snr, mCenter.array);  // 根据平均snr计算出来的加权平均质心
                // 打分，关联轨迹
                gtrack_moduleScore(inst, point, mCenter, allocNum);
                ret = inst->bestIndex[n] < inst->maxNumTracks;
                // 关联失败，分配轨迹
                if (!ret)
                {
                    gtrack_moduleAllocate(inst, point, mCenter, allocNum);
                    ret = inst->bestIndex[n] < inst->maxNumTracks;
                }
                // 关联或分配成功，记录关联点索引
                if (ret)
                {
                    idx = inst->bestIndex[n];
                    gUnit = (GtrackUnitInstance*)inst->hTrack[idx];
                    if (inst->associateNums[idx] != 0)
                    {
                        // 当前轨迹已关联过测量点，重新计算轨迹质心
                        mCenter.snr += gUnit->center.snr;
                        mCenter.doa_snr += gUnit->center.doa_snr;
                        gtrack_vectorScalarMulAcc(GTRACK_MEASUREMENT_VECTOR_SIZE, gUnit->center.array, gUnit->center.snr, mSum.array);
                        gtrack_vectorScalarMul(GTRACK_MEASUREMENT_VECTOR_SIZE, mSum.array, 1.0f / mCenter.snr, mCenter.array);
                    }
                    gUnit->center = mCenter;
                    memcpy(&inst->associateDatas[idx][inst->associateNums[idx]], inst->allocIndex, allocNum * sizeof(uint16_t));
                    inst->associateNums[idx] += allocNum;
                }
            }
        }
    }
}

void gtrack_moduleScore(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, GTRACK_measurementPoint center, uint16_t num)
{
    GTrack_ListElem* tElem;
    uint16_t uid;

    // 遍历当前所有活动tracker，执行每一个tracker的score函数
    tElem = gtrack_listGetFirst(&inst->activeList);
    while (tElem != 0)
    {
        uid = tElem->data;
        gtrack_unitScore(inst->hTrack[uid], point, inst->allocIndex, inst->bestScore, inst->bestIndex, center, num);
        tElem = gtrack_listGetNext(tElem);
    }
}

void gtrack_moduleAllocate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, GTRACK_measurementPoint center, uint16_t allocNum)
{
    uint16_t k;
    GTrack_ListElem* tElemFree;

    // 找出一个空闲tracker，没有则直接返回
    tElemFree = gtrack_listGetFirst(&inst->freeList);
    if (tElemFree == 0)
    {
        return;
    }
    //若聚类结果满足条件，则为其分配新tracker
    //首先判定聚类的点数和速度
    if ((allocNum > inst->params.allocationParams.pointsThre) &&
        (fabsf(center.vector.doppler) > inst->params.allocationParams.velocityThre))
    {
        // SNR条件满足，则分配新tracker
        if (center.snr > inst->params.allocationParams.snrThre)
        {
            // 将聚类中的所有点关联到新tracker UID
            for (k = 0; k < allocNum; k++)
            {
                inst->bestIndex[inst->allocIndex[k]] = (uint8_t)tElemFree->data;
                inst->bestScore[inst->allocIndex[k]] = 0;
            }
            // 分配新tracker
            inst->targetNumTotal++;
            inst->targetNumCurrent++;
            tElemFree = gtrack_listDequeue(&inst->freeList);   
            gtrack_unitStart(inst->hTrack[tElemFree->data], inst->heartBeat, inst->targetNumTotal, &center.vector, center.snr);
            inst->associateNums[tElemFree->data] = 0;
            gtrack_listEnqueue(&inst->activeList, tElemFree);
        }
    }
}

void gtrack_moduleUpdate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t num)
{
    GTrack_ListElem* tElem;
    uint16_t uid;

    tElem = gtrack_listGetFirst(&inst->activeList);
    while (tElem != 0)
    {
        uid = tElem->data;
        // 更新tracker
        gtrack_unitUpdate(inst->hTrack[uid], point, inst->bestScore, inst->associateDatas[uid], inst->associateNums[uid]);
        tElem = gtrack_listGetNext(tElem);
    }
}

void gtrack_moduleDelete(GtrackModuleInstance* inst)
{
    GTrack_ListElem* tElem;
    GTrack_ListElem* tElemToRemove;
    uint16_t         uid;

    tElem = gtrack_listGetFirst(&inst->activeList);
    while (tElem != 0)
    {
        uid = tElem->data;
        // 更新tracker
        GtrackUnitInstance* handle = (GtrackUnitInstance*)inst->hTrack[uid];
        // 删除空闲的tracker
        if (handle->state == TRACK_STATE_FREE)
        {
            tElemToRemove = tElem;
            tElem = gtrack_listGetNext(tElem);
            int res = gtrack_listRemoveElement(&inst->activeList, tElemToRemove);
            gtrack_unitStop(inst->hTrack[tElemToRemove->data]);
            inst->targetNumCurrent--;
            gtrack_listEnqueue(&inst->freeList, tElemToRemove);
        }
        else
            tElem = gtrack_listGetNext(tElem);
    }

}

void gtrack_moduleMerge(GtrackModuleInstance* inst)
{
    if (inst->heartBeat <= 1)
    {
        return;
    }

    uint16_t idx, i;
    GTrack_ListElem *tElem1, *tElem2;
    float sub[GTRACK_STATE_VECTOR_SIZE];
    float distThre = inst->params.mergeParams.mergeDistThre;
    float speedThre = inst->params.mergeParams.mergeSpeedThre;
    float countThre = inst->params.mergeParams.mergeCountThre;

    tElem1 = gtrack_listGetFirst(&inst->activeList);
    while (tElem1 != 0)
    {
        uint32_t uid1 = tElem1->data;
        GtrackUnitInstance* gUint1 = (GtrackUnitInstance*)inst->hTrack[uid1];
        tElem2 = gtrack_listGetNext(tElem1);
        while (tElem2 != 0)
        {
            uint32_t uid2 = tElem2->data;
            GtrackUnitInstance* gUint2 = (GtrackUnitInstance*)inst->hTrack[uid2];
            gtrack_vectorSub(GTRACK_STATE_VECTOR_SIZE, gUint1->S_hat, gUint2->S_hat, sub);
            float distSub2 = sub[0] * sub[0] + sub[1] * sub[1];
            float speedSub2 = sub[2] * sub[2] + sub[3] * sub[3];

            if (distSub2 <= distThre * distThre && speedSub2 <= speedThre * speedThre)
            {
                if (gUint1->state != gUint2->state)
                {
                    uint8_t res = !!(gUint1->state > gUint2->state);
                    gUint2->mergedCount[uid1] += res;
                    gUint1->mergedCount[uid2] += !res;
                }
                else
                {
                    if (gUint1->velocityHandling != gUint2->velocityHandling)
                    {
                        uint8_t res = !!((gUint1->velocityHandling > gUint2->velocityHandling));
                        gUint2->mergedCount[uid1] += res;
                        gUint1->mergedCount[uid2] += !res;
                    }
                    else
                    {
                        uint8_t res = !!((gUint1->allocationTime < gUint2->allocationTime));
                        gUint2->mergedCount[uid1] += res;
                        gUint1->mergedCount[uid2] += !res;
                    }
                }
            }
            else
            {
                gUint1->mergedCount[uid2] = 0;
                gUint2->mergedCount[uid1] = 0;
            }

            if (gUint2->mergedCount[uid1] >= countThre)
            {
                gUint2->state = TRACK_STATE_FREE;
                gUint1->mergedCount[uid2] = 0;
                for (i = 0; i < inst->associateNums[uid2]; i++)
                {
                    idx = inst->associateDatas[uid2][i];
                    inst->associateDatas[uid1][inst->associateNums[uid1]] = idx;
                    inst->associateNums[uid1]++;
                    inst->bestIndex[idx] = uid1;
                }
                inst->associateNums[uid2] = 0;
            }
            if (gUint1->mergedCount[uid2] >= countThre)
            {
                gUint1->state = TRACK_STATE_FREE;
                gUint2->mergedCount[uid1] = 0;
                for (i = 0; i < inst->associateNums[uid1]; i++)
                {
                    idx = inst->associateDatas[uid1][i];
                    inst->associateDatas[uid2][inst->associateNums[uid2]] = idx;
                    inst->associateNums[uid2]++;
                    inst->bestIndex[idx] = uid2;
                }
                inst->associateNums[uid1] = 0;
                break;
            }
            tElem2 = gtrack_listGetNext(tElem2);
        }
        tElem1 = gtrack_listGetNext(tElem1);
    }
}

void gtrack_moduleCalcAdaptiveEps(GtrackModuleInstance* inst, GTRACK_measurementPoint* points, uint16_t num_points, uint16_t k_neighbors)
{
    float distances[MAX_NEIGHBORS];
    float diffs[MAX_NEIGHBORS];
    const float max_dist_thre2 = inst->params.allocationParams.maxDistanceThre * inst->params.allocationParams.maxDistanceThre;
    for (int idx = 0; idx < num_points; idx++)
    {
        inst->epsIndex[idx] = max_dist_thre2;
        // 跳过已关联或低速点
        if (inst->bestIndex[idx] != GTRACK_ID_POINT_NOT_ASSOCIATED || fabsf(points[idx].vector.doppler) < inst->params.allocationParams.velocityThre)
        {
            continue;
        }

        const float max_dist2 = 4 * max_dist_thre2; // 初始距离限制到两倍的阈值范围内
        //const float max_dist = 20.f;
        const float SIGNIFICANCE_LEVEL = 2.0;      // 三倍标准差
        const int   WINDOW_SIZE = 3;

        k_neighbors = MIN(k_neighbors, MAX_NEIGHBORS);
        uint16_t d_size = 0;
        for (uint16_t j = 0; j < num_points; ++j)
        {
            if (inst->bestIndex[j] == GTRACK_ID_POINT_NOT_ASSOCIATED && fabsf(points[j].vector.doppler) > inst->params.allocationParams.velocityThre)
            {
                float dist2 = (gtrack_calcDistance(&points[idx].vector, &points[j].vector)); // 计算距离
                if (dist2 < max_dist2)
                {
                    if (d_size > MAX_NEIGHBORS)
                    {
                        break;
                    }
                    distances[d_size++] = dist2;
                }
            }
        }

        if (d_size < k_neighbors)
        {
            continue;
        }

        float temp;
        int swapped = 0;  // 标记是否发生交换
        //使用冒泡排序Top-K 
        for (uint16_t id = 0; id < k_neighbors; id++)
        {
            for (uint16_t m = d_size - 1; m > id; --m)
            {
                if (distances[m] < distances[m - 1])
                {
                    temp = distances[m - 1];
                    distances[m - 1] = distances[m];
                    distances[m] = temp;
                    swapped = 1;
                }
            }
            if (!swapped)
            {
                break; // 提前终止：若未交换，说明已有序
            }
        }

        if (distances[1] > max_dist_thre2) // 最近的点都超过了阈值，直接pass
        {
            continue;
        }

        //开始滑窗计算
        int window_start = 2, first_window_end = window_start + WINDOW_SIZE - 1;
        // 统计量计算
        float sum = 0.0f, sq_sum = 0.0f, mean = 0.f, variance = 0.f, threshold = 0.f;
        for (int i = window_start; i < k_neighbors; i++)
        {
            if (distances[i] > max_dist_thre2)
            {
                inst->epsIndex[idx] = MIN(distances[i - 1], max_dist_thre2);
                break;
            }
            diffs[i] = distances[i] - distances[i - 1];
            sum += diffs[i];
            sq_sum += diffs[i] * diffs[i];
            if (i >= first_window_end)
            {
                if (i > first_window_end)
                {
                    // 异常跳变检测
                    for (uint8_t m = i - WINDOW_SIZE + 1; m <= i; ++m)
                    {
                        if (diffs[m] > threshold)
                        {
                            /* 更新epsIndex */
                            inst->epsIndex[idx] = MIN(distances[m - 1], max_dist_thre2);
                            // printf("在迭代后跳出 第 [%d] 个点 选择第 [%d] 个点 半径 ：[%f]-[%f]此时分隔点距离：%f  \n",i, optimal_idx, inst->epsIndex[i], distances[optimal_idx], distances[optimal_idx + 1]);
                            //printf("此时window_start = %d ， m= :%d,diffs  = %f - %f - %f, threshold:%f\n ", window_start,m, diffs[0], diffs[1], diffs[2], threshold);
                            break;
                        }
                    }
                    sum -= diffs[i - WINDOW_SIZE];
                    sq_sum -= diffs[i - WINDOW_SIZE] * diffs[i - WINDOW_SIZE];
                }
                //todo:这里想法是放宽最近点限制，缩小次宽点以后的限制，目的是在范围内尽量扩大聚类半径，在点集内部缩小半径，以区分密集簇，聚类离散点簇 举例：一个点最近的点2m远，但里他最近的点附近有很多密集点，
                // 距离都稍远一点，若另外点云簇足够密集，自然会排除它，若不够密集，则认为该点可能是因为噪声产生的分离，只要满足速度等特征一致仍可聚类；
                // 但这么做是否真的足够合理
                mean = sum / WINDOW_SIZE;
                variance = (sq_sum / WINDOW_SIZE) - (mean * mean);
                variance = MAX(variance, 0.0f);
                threshold = mean + SIGNIFICANCE_LEVEL * sqrtf(variance);
            }
        }
    }
}

void gtrack_moduleReport(GtrackModuleInstance* inst, GTRACK_targetDesc* t, uint16_t* tNum, GTRACK_measurementPoint* point, uint16_t mNum)
{
    GTrack_ListElem* tElem;
    uint16_t         uid;
    uint16_t         num = 0;
    tElem = gtrack_listGetFirst(&inst->activeList);
    while (tElem != 0)
    {
        uid = tElem->data;
        gtrack_unitReport(inst->hTrack[uid], &t[num++]);
        GtrackUnitInstance* gUint = (GtrackUnitInstance*)inst->hTrack[uid];
        printf_message(ROUGH_RESULT_PRINT, "[RESULT] Trajectory [%d] (UID: %d) - Coords: (%f, %f), State Def: [%d], Stationary: [%d], Speed Proc Status: [%d], SNR([%f] - [%f])\n", gUint->tid, uid, gUint->S_hat[0], gUint->S_hat[1], gUint->state, gUint->isTargetStatic, gUint->velocityHandling, gUint->center.snr, gUint->allocSNR);
        printf_message(DETAILED_PROCESS_PRINT, "         detect2activeCount：[%d], detect2freeCount: [%d],active2freeCount: [%d] \n", gUint->detect2activeCount, gUint->detect2freeCount, gUint->active2freeCount);
        printf_message(DETAILED_RESULT_PRINT, "         Estimated speed: [%f], Est. range: [%f], Est. angle: [%f], Calc. speed: [%f], Curr. est. point count: [%f]\n", gUint->H_s.vector.doppler, gUint->H_s.vector.range, gUint->H_s.vector.angle, gUint->rangeRate, gUint->estNumOfPoints);
        for (int idx = 0; idx < mNum; idx++)
        {
            if (inst->bestIndex[idx] == uid)
            {
                printf_message(DETAILED_RESULT_PRINT, "	 Points No.[%d] range:[%f],angle:[%f],doppler:[%f],snr:[%f], score:[%f]\n", idx, point[idx].vector.range, point[idx].vector.angle, point[idx].vector.doppler, point[idx].snr, inst->bestScore[idx]);
            }
        }
        tElem = gtrack_listGetNext(tElem);
    }
    *tNum = num;
}
